#ifndef ROTATIONAVERAGING_H_INCLUDED
#define ROTATIONAVERAGING_H_INCLUDED

#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <algorithm>
#include <vector>
#include <cmath>
#include <map>
#include <queue>
#include <numeric>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <Eigen/Cholesky>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include "estimators/l1_solver.h"
#include "util/func.h"

using namespace Eigen;
using namespace std;
using namespace boost;

typedef Eigen::Matrix<double, 3, 3> Mat3;
typedef Eigen::Matrix<double, 3, 1> Vec3;
typedef Eigen::Matrix<double, 4, 1> Vec4;
typedef Eigen::Matrix<double, 5, 1> Vec5;
typedef Eigen::MatrixXd Mat;
typedef Eigen::VectorXd Vec;
typedef uint32_t IndexT;
typedef double REAL;
typedef std::vector<Mat3> Matrix3x3Arr;

/// Sparse unconstrained matrix using double internal format
typedef Eigen::SparseMatrix<double> sMat;

/// Sparse unconstrained matrix using double internal format and Row Major storage
typedef Eigen::SparseMatrix<double, Eigen::RowMajor> sRMat;

typedef Mat3 Matrix3x3;
typedef std::vector<size_t> IndexArr;

struct Node
{
  typedef IndexArr InternalType;
  InternalType edges; // array of vertex indices
};

template<typename T>
inline T Square(T x)
{
    return x * x;
}
typedef std::vector<Node> NodeArr;

struct Link
{
  size_t ID; // node index
  size_t parentID;// parent link
  inline Link(size_t _ID=0, size_t _parentID=0) : ID(_ID), parentID(_parentID) {}
};
typedef std::queue<Link> LinkQue;

typedef boost::property<boost::edge_weight_t, float> edge_property_t;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, size_t, edge_property_t> graph_t;
typedef graph_t::vertex_descriptor vertex_t;
typedef graph_t::edge_descriptor edge_t;
typedef boost::graph_traits<graph_t>::edge_iterator edge_iter;

struct RelativeRotation
{
  IndexT i, j; // pose's indices
  Mat3 Rij; // pose's relative rotation
  float weight;

  RelativeRotation(IndexT i_=0, IndexT j_=0, const Mat3 & Rij_=Mat3::Identity(), float weight_=1.0f):
  i(i_), j(j_), Rij(Rij_), weight(weight_)
  {}
};

typedef std::vector<RelativeRotation> RelativeRotations;
typedef std::map<std::pair<size_t,size_t>, Matrix3x3> MapEdgeIJ2R;

template<typename TMat>
inline double FrobeniusNorm(const TMat &A)
{
    return sqrt(A.array().abs2().sum());
}

template <typename T>
inline void AngleAxisToRotationMatrix(const T * angle_axis, T * R)
{
  static const T kOne = T(1.0);
  const T theta2 = angle_axis[0] * angle_axis[0] + angle_axis[1] * angle_axis[1] + angle_axis[2] * angle_axis[2];
  if (theta2 > 0.0)
  {
    const T theta = sqrt(theta2);
    const T wx = angle_axis[0] / theta;
    const T wy = angle_axis[1] / theta;
    const T wz = angle_axis[2] / theta;

    const T costheta = cos(theta);
    const T sintheta = sin(theta);

    R[0] =     costheta   + wx*wx*(kOne -    costheta);
    R[1] =  wz*sintheta   + wx*wy*(kOne -    costheta);
    R[2] = -wy*sintheta   + wx*wz*(kOne -    costheta);
    R[3] =  wx*wy*(kOne - costheta)     - wz*sintheta;
    R[4] =     costheta   + wy*wy*(kOne -    costheta);
    R[5] =  wx*sintheta   + wy*wz*(kOne -    costheta);
    R[6] =  wy*sintheta   + wx*wz*(kOne -    costheta);
    R[7] = -wx*sintheta   + wy*wz*(kOne -    costheta);
    R[8] =     costheta   + wz*wz*(kOne -    costheta);
  }
  else
  {
    R[0] =  kOne;
    R[1] = -angle_axis[2];
    R[2] =  angle_axis[1];
    R[3] =  angle_axis[2];
    R[4] =  kOne;
    R[5] = -angle_axis[0];
    R[6] = -angle_axis[1];
    R[7] =  angle_axis[0];
    R[8] = kOne;
  }
}
template <typename Type, typename DataInputIterator>
bool minMaxMeanMedian( DataInputIterator begin, DataInputIterator end,
                       Type & min, Type & max, Type & mean, Type & median )
{
  if( std::distance( begin, end ) < 1 )
  {
    return false;
  }

  std::vector<Type> vec_val( begin, end );
  std::sort( vec_val.begin(), vec_val.end() );
  min = vec_val[0];
  max = vec_val[vec_val.size() - 1];
  mean = accumulate( vec_val.begin(), vec_val.end(), Type( 0 ) )
         / static_cast<Type>( vec_val.size() );
  median = vec_val[vec_val.size() / 2];
  return true;
}

double RelRotationAvgError
(
  const RelativeRotations& RelRs,
  const Matrix3x3Arr& Rs,
  double* pMin=NULL,
  double* pMax=NULL
)
{
  std::vector<double> vec_err(RelRs.size(), 0.0);
  for (size_t i=0; i < RelRs.size(); ++i) {
    const RelativeRotation& relR = RelRs[i];
    vec_err[i] = FrobeniusNorm(relR.Rij  - (Rs[relR.j]*Rs[relR.i].transpose()));
  }
  float min, max, mean, median;
  minMaxMeanMedian(vec_err.begin(), vec_err.end(), min, max, mean, median);
  if (pMin)
    *pMin = min;
  if (pMax)
    *pMax = max;
  return mean;
}

template <typename T>
inline void RotationMatrixToAngleAxis(const T * R, T * angle_axis)
{
  // x = k * 2 * sin(theta), where k is the axis of rotation.
  angle_axis[0] = R[5] - R[7];
  angle_axis[1] = R[6] - R[2];
  angle_axis[2] = R[1] - R[3];

  static const T kOne = T(1.0);
  static const T kTwo = T(2.0);

  T costheta = std::min(std::max((R[0] + R[4] + R[8] - kOne) / kTwo,
                                 T(-1.0)),
                        kOne);

  T sintheta = std::min(sqrt(angle_axis[0] * angle_axis[0] +
                             angle_axis[1] * angle_axis[1] +
                             angle_axis[2] * angle_axis[2]) / kTwo,
                        kOne);

  // Use the arctan2 to get the right sign on theta
  const T theta = atan2(sintheta, costheta);

  static const double kThreshold = 1e-12;
  if ((sintheta > kThreshold) || (sintheta < -kThreshold)) {
    const T r = theta / (kTwo * sintheta);
    for (int i = 0; i < 3; ++i) {
      angle_axis[i] *= r;
    }
    return;
  }

  // Case 2: theta ~ 0, means sin(theta) ~ theta to a good
  // approximation.
  if (costheta > 0.0) {
    const T kHalf = T(0.5);
    for (int i = 0; i < 3; ++i) {
      angle_axis[i] *= kHalf;
    }
    return;
  }

  const T inv_one_minus_costheta = kOne / (kOne - costheta);
  for (int i = 0; i < 3; ++i) {
    angle_axis[i] = theta * sqrt((R[i*4] - costheta) * inv_one_minus_costheta);
    if (((sintheta < 0.0) && (angle_axis[i] > 0.0)) ||
        ((sintheta > 0.0) && (angle_axis[i] < 0.0))) {
      angle_axis[i] = -angle_axis[i];
    }
  }
}

unsigned int FindConsistentRelativeRotations(
  const RelativeRotations& RelRs,
  const Matrix3x3Arr& Rs,
  float threshold,
  RelativeRotations& RelRsNew,
  std::vector<float>& errors)
{
  assert(!RelRs.empty() && !Rs.empty());
  assert(threshold >= 0);

  // compute errors for each relative rotation
  errors.resize(RelRs.size());
  for (size_t r= 0; r<RelRs.size(); ++r) {
    const RelativeRotation& relR = RelRs[r];
    const Matrix3x3& Ri = Rs[relR.i];
    const Matrix3x3& Rj = Rs[relR.j];
    const Matrix3x3& Rij = relR.Rij;
    const Mat3 eRij(Rj.transpose()*Rij*Ri);

    const Vec3 erij;
    RotationMatrixToAngleAxis((const double*)eRij.data(), (double*)erij.data());
    errors[r] = (float)(erij.norm() * 180.0 / 3.141592653);
  }

  std::vector<float> errorsO(errors.begin(), errors.end());
  double meanError = 0.0, medianError = 0.0;
  double stderror = 0.0;
  for (size_t r=0; r< errorsO.size(); ++r)
  {
    meanError += errorsO[r] / errorsO.size();
  }
  for (size_t r=0; r< errorsO.size(); ++r)
  {
    stderror += (errorsO[r] - meanError) * (errorsO[r] - meanError) / errorsO.size();
  }
  stderror = sqrt(stderror);
  std::sort(errorsO.begin(), errorsO.end());
  medianError = errorsO[errorsO.size() / 2];
  LOG(INFO) << "Mean Error For Filtering: " << meanError << ", Median Error For Filtering: " << medianError << std::endl;
  LOG(INFO) << "STD Error For Filtering: " << stderror << std::endl;
  LOG(INFO) << "Threshold: " << threshold << std::endl;

  // threshold = meanError + 3.0 * stderror;
  // mark outliers
  unsigned int nInliers = 0;
  RelRsNew.clear();
  for (size_t r=0; r<errors.size(); ++r)
  {
    if (errors[r] < threshold)
    {
        RelRsNew.push_back(RelRs[r]);
        ++nInliers;
    }
  }
  return nInliers;
} // FilterRelativeRotations

// build A in Ax=b
inline void FillMappingMatrix(
  const RelativeRotations& RelRs,
  const size_t nMainViewID,
  sMat& A)
{
  A.reserve(A.rows()*2); // estimate of the number of non-zeros (optional)
  sMat::Index i = 0, j = 0;
  for (size_t r=0; r<RelRs.size(); ++r) {
    const RelativeRotation& relR = RelRs[r];
    if (relR.i != nMainViewID) {
      j = 3*(relR.i<nMainViewID ? relR.i : relR.i-1);
      A.insert(i+0,j+0) = -1.0;
      A.insert(i+1,j+1) = -1.0;
      A.insert(i+2,j+2) = -1.0;
    }
    if (relR.j != nMainViewID) {
      j = 3*(relR.j<nMainViewID ? relR.j : relR.j-1);
      A.insert(i+0,j+0) = 1.0;
      A.insert(i+1,j+1) = 1.0;
      A.insert(i+2,j+2) = 1.0;
    }
    i+=3;
  }
  A.makeCompressed();
}

// compute errors for each relative rotation
inline void FillErrorMatrix(
  const RelativeRotations& RelRs,
  const Matrix3x3Arr& Rs,
  Vec & b)
{
  for (size_t r = 0; r < RelRs.size(); ++r) {
    const RelativeRotation& relR = RelRs[r];
    const Matrix3x3& Ri = Rs[relR.i];
    const Matrix3x3& Rj = Rs[relR.j];
    const Matrix3x3& Rij = relR.Rij;
    const Mat3 eRij(Rj.transpose()*Rij*Ri);
    const Vec3 erij;
    RotationMatrixToAngleAxis((const double*)eRij.data(), (double*)erij.data());
    b.block<3,1>(3*r,0) = erij;
  }
}

// apply correction to global rotations
inline void CorrectMatrix(
  const Mat& x,
  const size_t nMainViewID,
  Matrix3x3Arr& Rs)
{
  for (size_t r = 0; r < Rs.size(); ++r) {
    if (r == nMainViewID)
      continue;
    Matrix3x3& Ri = Rs[r];
    const size_t i = (r<nMainViewID ? r : r-1);
    const Vec3 eRid = Vec3(x.block<3,1>(3*i,0));
    const Mat3 eRi;
    AngleAxisToRotationMatrix((const double*)eRid.data(), (double*)eRi.data());
    Ri = Ri*eRi;
  }
}

// L1RA -> L1 Rotation Averaging implementation
bool SolveL1RA
(
  const RelativeRotations& RelRs,
  Matrix3x3Arr& Rs,
  const sMat & A,
  const unsigned int nMainViewID
)
{
  const unsigned nObss = (unsigned)RelRs.size();
  const unsigned nVars = (unsigned)Rs.size()-1; // one view is kept constant
  const unsigned m = nObss*3;
  const unsigned n = nVars*3;

  // init x with 0 that corresponds to trusting completely the initial Ri guess
  Vec x(Vec::Zero(n)), b(m);

  // Current error and the previous one
  double e = std::numeric_limits<double>::max(), ep;
  unsigned iter = 0;
  // L1RA iterate optimization till the desired precision is reached
  do {
    // compute errors for each relative rotation
    FillErrorMatrix(RelRs, Rs, b);

    // solve the linear system using l1 norm
    L1Solver<sMat >::Options options;
    L1Solver<sMat > l1_solver(options, A);
    l1_solver.Solve(b, &x);

    ep = e; e = x.norm();
    if (ep < e)
      break;
    // apply correction to global rotations
    CorrectMatrix(x, nMainViewID, Rs);
  } while (++iter < 32 && e > 1e-5 && (ep-e)/e > 1e-2);

  LOG(INFO) << "L1RA Converged in " << iter << " iterations." << std::endl;

  return true;
}

// Iteratively Reweighted Least Squares (IRLS) implementation
bool SolveIRLS
(
  const RelativeRotations& RelRs,
  Matrix3x3Arr& Rs,
  const sMat & A,
  const unsigned int nMainViewID,
  const double sigma
)
{
  const unsigned nObss = (unsigned)RelRs.size();
  const unsigned nVars = (unsigned)Rs.size()-1; // one view is kept constant
  const unsigned m = nObss*3;
  const unsigned n = nVars*3;

  // init x with 0 that corresponds to trusting completely the initial Ri guess
  Vec x(Vec::Zero(n)), b(m);

  // Since the sparsity pattern will not change with each linear solve
  //  compute it once to speed up the solution time.
  typedef Eigen::SimplicialLDLT<sMat > Linear_Solver_T;

  Linear_Solver_T linear_solver;
  linear_solver.analyzePattern(A.transpose() * A);
  if (linear_solver.info() != Eigen::Success) {
    LOG(ERROR) << "Cholesky decomposition failed." << std::endl;
    return false;
  }

  const double sigmaSq(Square(sigma));

  Eigen::ArrayXd errors, weights;
  Vec xp(n);
  // current error and the previous one
  double e = std::numeric_limits<double>::max(), ep;
  unsigned int iter = 0;
  do
  {
    xp = x;
    // compute errors for each relative rotation
    FillErrorMatrix(RelRs, Rs, b);

    // Compute the weights for each error term
    errors = (A * x - b).array();

    // compute robust errors using the Huber-like loss function
    weights = sigmaSq / (errors.square() + sigmaSq).square(); /// ICCV 2013

    // weights = 1 / (Eigen::pow(errors.square(), 3/8)); /// PAMI2017

    // Update the factorization for the weighted values
    const sMat at_weight = A.transpose() * weights.matrix().asDiagonal();
    linear_solver.factorize(at_weight * A);
    if (linear_solver.info() != Eigen::Success) {
      LOG(ERROR) << "Failed to factorize the least squares system." << std::endl;
      return false;
    }

    // Solve the least squares problem
    x = linear_solver.solve(at_weight * b);
    if (linear_solver.info() != Eigen::Success) {
      LOG(ERROR) << "Failed to solve the least squares system." << std::endl;
      return false;
    }

    // apply correction to global rotations
    CorrectMatrix(x, nMainViewID, Rs);

    ep = e; e = (xp-x).norm();

  } while (++iter < 32 && e > 1e-5 && (ep-e)/e > 1e-2);

  LOG(INFO) << "IRLS Converged in " << iter << " iterations." << std::endl;

  return true;
}

// Refine the global rotations using to the given relative rotations, similar to:
// "Efficient and Robust Large-Scale Rotation Averaging", Chatterjee and Govindu, 2013
// L1 Rotation Averaging (L1RA) and Iteratively Reweighted Least Squares (IRLS) implementations combined
bool RefineRotationsAvgL1IRLS(const RelativeRotations& RelRs,
  const RelativeRotations& RelRsFilter, Matrix3x3Arr& Rs,
  const size_t nMainViewID,
  const double sigma)
{
  assert(!RelRs.empty() && !Rs.empty());
  assert(Rs[nMainViewID] == Matrix3x3::Identity());

  double fMinBefore, fMaxBefore;
  const double fMeanBefore = RelRotationAvgError(RelRsFilter, Rs, &fMinBefore, &fMaxBefore);

  const unsigned nObssL1 = (unsigned)RelRsFilter.size();
  const unsigned nVarsL1 = (unsigned)Rs.size()-1; // main view is kept constant
  const unsigned mL1 = nObssL1*3;
  const unsigned nL1 = nVarsL1*3;

  // build mapping matrix A in Ax=b
  sMat AL1(mL1, nL1);
  FillMappingMatrix(RelRsFilter, nMainViewID, AL1);

  if (!SolveL1RA(RelRsFilter, Rs, AL1, nMainViewID))
  {
    LOG(ERROR) << "Could not solve the L1 regression step." << std::endl;
    return false;
  }

  double fMinMid, fMaxMid;
  const double fMeanMid = RelRotationAvgError(RelRs, Rs, &fMinMid, &fMaxMid);

  LOG(INFO) << "Refine global rotations using L1RA " << nObssL1 << " relative rotations:\n"
    << " error reduced from " << fMeanBefore << "(" << fMinBefore << " min, " << fMaxBefore << " max)\n"
    << " to " << fMeanMid << "(" << fMinMid << "min,"<< fMaxMid<< "max)" << std::endl;

  RelativeRotations RelRsNew;
  std::vector<float> errors;
  LOG(INFO) << FindConsistentRelativeRotations(RelRs, Rs, 170, RelRsNew, errors) << " inliers after L1RA" << std::endl; /// maybe not robust since the MST may lost

  const unsigned nObssL2 = (unsigned)RelRsNew.size();
  const unsigned nVarsL2 = (unsigned)Rs.size()-1; // main view is kept constant
  const unsigned mL2 = nObssL2*3;
  const unsigned nL2 = nVarsL2*3;

  // build mapping matrix A in Ax=b
  sMat AL2(mL2, nL2);
  FillMappingMatrix(RelRsNew, nMainViewID, AL2);

  if (!SolveIRLS(RelRsNew, Rs, AL2, nMainViewID, sigma))
  {
    LOG(ERROR) << "Could not solve the ILRS step." << std::endl;
    return false;
  }

  double fMinAfter, fMaxAfter;
  const double fMeanAfter = RelRotationAvgError(RelRs, Rs, &fMinAfter, &fMaxAfter);

  LOG(INFO) << "Refine global rotations using IRLS " << nObssL2 << " relative rotations:\n"
    << " error reduced from " << fMeanMid << "(" << fMinMid << " min, " << fMaxMid << " max)\n"
    << " to " << fMeanAfter << "(" << fMinAfter << "min,"<< fMaxAfter<< "max)" << std::endl;

  return true;
} // RefineRotationsAvgL1IRLS

size_t FindMaximumSpanningTree(const RelativeRotations& RelRs, graph_t& g, MapEdgeIJ2R& mapIJ2R, NodeArr& minGraph)
{
  assert(!RelRs.empty());
  for (size_t p = 0; p < RelRs.size(); ++p) {
    const RelativeRotation& relR = RelRs[p];
    boost::add_edge(relR.i, relR.j, - relR.weight, g);
    mapIJ2R[std::make_pair(relR.i, relR.j)] = relR.Rij;
    mapIJ2R[std::make_pair(relR.j, relR.i)] = relR.Rij.transpose();
  }
  // find the minimum spanning tree
  const size_t nViews = boost::num_vertices(g);
  minGraph.resize(nViews);
  std::vector<edge_t> spanningTree;
  boost::kruskal_minimum_spanning_tree(g, std::back_inserter(spanningTree));
  for (std::vector<edge_t>::const_iterator ei=spanningTree.begin(); ei!=spanningTree.end(); ++ei) {
    const edge_t& edge = *ei;
    minGraph[edge.m_source].edges.push_back(edge.m_target);
    minGraph[edge.m_target].edges.push_back(edge.m_source);
  }
  const size_t nEdges = spanningTree.size();
  return nEdges;

}

void InitRotationsMST
(
  const RelativeRotations& RelRs,
  Matrix3x3Arr& Rs,
  const size_t nMainViewID
)
{
  assert(!Rs.empty());

  // -- Compute coarse global rotation estimates:
  //   - by finding the maximum spanning tree and linking the relative rotations
  //   - Initial solution is driven by relative rotations data confidence.
  graph_t g;
  MapEdgeIJ2R mapIJ2R;
  NodeArr minGraph;
  // find the Maximum Spanning Tree
  FindMaximumSpanningTree(RelRs, g, mapIJ2R, minGraph);
  g.clear();

  // start from the main view and link all views using the relative rotation estimates
  LinkQue stack;
  stack.push(Link(nMainViewID, size_t(0)));
  Rs[nMainViewID] = Matrix3x3::Identity();
  do {
    const Link& link = stack.front();
    const Node& node = minGraph[link.ID];

    for(Node::InternalType::const_iterator pEdge = node.edges.begin();
      pEdge != node.edges.end(); ++pEdge) {
        const size_t edge = *pEdge;
        if (edge == link.parentID) {
          // compute the global rotation for the current node
          assert(mapIJ2R.find(std::make_pair(link.parentID, link.ID)) != mapIJ2R.end());
          const Matrix3x3& Rij = mapIJ2R[std::make_pair(link.parentID, link.ID)];
          Rs[link.ID] = Rij * Rs[link.parentID];
        } else {
          // add edge to the processing queue
          stack.push(Link(edge, link.ID));
        }
    }
    stack.pop();
  } while(!stack.empty());
}

template<typename TYPE>
inline std::pair<TYPE, TYPE>
ComputeX84Threshold(const TYPE* const values, size_t size, TYPE mul=TYPE(5.2))
{
  assert(size > 0);
  typename std::vector<TYPE> data(values, values+size);
  typename std::vector<TYPE>::iterator mid = data.begin() + size / 2;
  std::nth_element(data.begin(), mid, data.end());
  const TYPE median = *mid;
  // threshold = 5.2 * MEDIAN(ABS(values-median));
  for (size_t i=0; i<size; ++i)
    data[i] = std::abs(values[i]-median);
  std::nth_element(data.begin(), mid, data.end());
  return std::make_pair(median, mul*(*mid));
} // ComputeX84Threshold

unsigned int FilterRelativeRotations(
  const RelativeRotations& RelRs,
  const Matrix3x3Arr& Rs,
  float threshold,
  std::vector<bool> * vec_inliers,
  std::vector<double>& error)
{
  assert(!RelRs.empty() && !Rs.empty());
  assert(threshold >= 0);
  // compute errors for each relative rotation
  std::vector<float> errors(RelRs.size());
  double errorSum = 0;
  for (size_t r= 0; r<RelRs.size(); ++r) {
    const RelativeRotation& relR = RelRs[r];
    const Matrix3x3& Ri = Rs[relR.i];
    const Matrix3x3& Rj = Rs[relR.j];
    const Matrix3x3& Rij = relR.Rij;
    const Mat3 eRij(Rj.transpose()*Rij*Ri);
    const Vec3 erij;
    RotationMatrixToAngleAxis((const double*)eRij.data(), (double*)erij.data());
    errors[r] = (float)erij.norm() * 180.0 / 3.141592653;
    error.push_back(errors[r]);
    errorSum += errors[r];
  }
  if (threshold == 0) {
    // estimate threshold
    const std::pair<float,float> res = ComputeX84Threshold(&errors[0], errors.size());
    threshold = res.first+res.second;
  }
  if (vec_inliers)  {
    vec_inliers->resize(RelRs.size());
  }
  // mark outliers
  unsigned int nInliers = 0;
  for (size_t r=0; r<errors.size(); ++r) {
    const bool bInlier = (errors[r] < threshold);
    if (vec_inliers)
      (*vec_inliers)[r] = bInlier;
    if (bInlier)
      ++nInliers;
  }
  std::sort(errors.begin(), errors.end());
  LOG(INFO) << "Epipolar edge relative rotation error, mean: " << errorSum / RelRs.size() << ", median: " << errors[errors.size()/2] << std::endl;
  LOG(INFO) << "Threshold: " << threshold << ", with " << nInliers << " inliers"<< std::endl;
  return nInliers;
} // FilterRelativeRotations


double ComRotationError(const Matrix3x3& R12, const Matrix3x3 R12_5points)
{
    double row0 = R12.row(0) * R12_5points.transpose().col(0);
    double row1 = R12.row(1) * R12_5points.transpose().col(1);
    double row2 = R12.row(2) * R12_5points.transpose().col(2);
    double acosValue = (row0 + row1 + row2 - 1.0 ) / 2.0;
    acosValue = acosValue > 1.0 ? 1.0 : acosValue;
    acosValue = acosValue < -1.0 ? -1.0 : acosValue;
    return acos(acosValue) * 180 / 3.141592653;
}

bool GlobalRotationsRobust(const RelativeRotations& RelRs,  Matrix3x3Arr& Rs, const size_t nMainViewID, float threshold,
                           std::vector<bool> * vec_Inliers, std::vector<double>& errorRR)
{
  assert(!Rs.empty());
  // -- Compute coarse global rotation estimates:
  InitRotationsMST(RelRs, Rs, nMainViewID);

  RelativeRotations RelRsNew;
  std::vector<float> errors;
  LOG(INFO) << FindConsistentRelativeRotations(RelRs, Rs, 180, RelRsNew, errors) << " inliers when initialized with MST" << std::endl;


  /// perform L1+L2 rotation averaging
  bool refineOK = true;
  if(RelRs.size() < (size_t)(1000))
  {
      double sigmaThreshold = 5.0*3.141592653/180.0;
      refineOK = RefineRotationsAvgL1IRLS(RelRs, RelRsNew, Rs, nMainViewID, sigmaThreshold);
  }

  std::vector<double> e_RL1L2(RelRs.size());
  for (size_t r = 0; r < RelRs.size(); ++r)
  {
    const RelativeRotation& RR = RelRs[r];
    const Matrix3x3& Ri = Rs[RR.i];
    const Matrix3x3& Rj = Rs[RR.j];
    const Matrix3x3& ErrorR = Rj*Ri.transpose() * RR.Rij.transpose();
    const Mat3 eRij(ErrorR);
    const Vec3 erij;
    RotationMatrixToAngleAxis((const double*)eRij.data(), (double*)erij.data());
    e_RL1L2[r] = (float)(erij.norm() * 180.0 / 3.141592653);
  }

  {
    if (threshold>=0 && vec_Inliers)
    {
        FilterRelativeRotations(RelRs, Rs, threshold, vec_Inliers, e_RL1L2);
    }
  }

  return refineOK;
} // GlobalRotationsRobust

void WriteResult(Matrix3x3Arr& Rs, char* filename)
{
    FILE* fout = fopen(filename, "w+");
    fprintf(fout, "%d\n", (int)Rs.size());
    for(int iR = 0; iR < (int)Rs.size(); iR++)
    {
        for(int ir = 0; ir < 3; ir++)
            fprintf(fout, "%lf %lf %lf ", Rs[iR](ir, 0), Rs[iR](ir, 1), Rs[iR](ir, 2));
        fprintf(fout, "\n");
    }
    fclose(fout);
}
#endif // ROTATIONAVERAGING_H_INCLUDED
